#PySwarm Interface Script
##########################################################################################
# TITLE
# PySwarm - A PyCarrara Script for Carrara
# Version 0.6
##########################################################################################
# DESCRIPTION
# PySwarm for Carrara is a Python script specifically written to use with PyCarraras
# plug-in for Carrara software to manipulate and render (in Carrara) realistic swarming 
# animals (generally called "BOIDs") and other similar forms of group behavior.
# The ultimate goal of this script is to have the capability of creating realistic animated
# boids (complex objects) flying or moving within a Carrara scene. The code is also 
# designed for reuse, so it will be easier to create new animation sequences using flocking 
# behaviors.
##########################################################################################
# LICENSING
# The PySwarm script shell is released for public use as Open Source Software
# For details, visit: http://opensource.org/osd
##########################################################################################

##########################################################################################
# Import external components - Do not modify these as they are used by the script
#=========================================================================================
import c3d
from PyCarrara import *
from math import *
from random import *
from pprint import *
from datetime import *
import PyCarrara

##########################################################################################

##########################################################################################
# Initialize constants - Do not modify these as they are used by the script
#=========================================================================================
conv = 2.54/100			# This is used to convert meter units into PyCarrara's inches
twopi = 2.0 * pi
r2d = 180.0 / pi		# Used for converting radians to degrees

# rotation_str is used to get and set an object's rotation
rotation_str = "/Instance/Motion Method/Transform/Rotation"
scaling_str = "/Instance/Motion Method/Transform/Scaling"


##########################################################################################
#***Insert parameters***


#***End of parameters insertion***
##########################################################################################

def magmin(a,b):
# This function returns the value a or b with the smallest magnitude
	#print 'magmin called with ', a*conv, b*conv
	if abs(a) < abs(b):
		return a
	else:
		return b

##########################################################################################
# DEBUGGING feature
#=========================================================================================
debugging = False

def start_debug():
	if debugging:
		global debugfile
		debugfile = open('debug.txt', 'w')
		print 'Debugging started'

def debug_line(*s):
# This function prints the string s to the debug file if debugging is turned on
	if debugging:
		debugfile.write(str(s)+'\n')

def end_debug():
	if debugging:
		debugfile.close()
		print 'Debugging ended'

##########################################################################################
# CLASSES - Do not modify these as they are used by the script
#=========================================================================================
class ModeError(Exception):
	def __init__(self, msg):
		self.errormsg = msg

class ObjectError(Exception):
	def __init__(self, msg):
		self.errormsg = msg
		print msg

def shifting(v):
#=========================================================================================
# This function slightly shifts the provided Vector to force PyCarrara and Carrara to write
# a keyframe for positioning and rotations when the BOID has not moved or rotated since the
# last keyframe entry.
#=========================================================================================
	v.x += random()/1000000.0
	v.y += random()/1000000.0
	v.z += random()/1000000.0
	return v

##########################################################################################
class CONTAINER:
#=========================================================================================
# This class handles a container object, used as part of BOID Rule #4.
#=========================================================================================

	#====================================================================================
	def __init__(self, safemode, type, cname, center, radius, height, size, adj):
	# This method initializes the Containers instance.
	# It also converts feet units into internal inch units.
	#------------------------------------------------------------------------------------

		self.container_safemode = safemode
		self.container_type = type
		self.container_name = cname
		if cname != '':
			try:
				self.container_c3dptr = c3d.Object('/'+cname)
			except:
				raise ObjectError('***PySwarm was unable to find a container named ' + cname + '***')
			self.container_scale = self.container_c3dptr.get_anim_param_value(current_time, scaling_str)
			if self.container_scale == 0.0:
				self.container_scale = 1.0
			#print 'Container scale for ',cname,' is ',self.container_scale
		else:
			self.container_scale = 1.0

		# Base is the center, so need to convert the location to "base"
		if type == 'B':
			base = center - (size / 2.0)
		elif type == 'C':
			base = center
			base.z = size.z / 2.0
		elif type == 'S':
			base = center
		self.container_base = base / conv
		self.container_radius = radius / conv
		self.orig_radius = radius / conv
		self.container_height = height / conv
		self.orig_height = height / conv
		self.container_size = size / conv
		self.orig_size = size / conv
		self.container_adj = adj

	#====================================================================================
	def update(self):
	# This method updates the container's variables if they are defined by a Carrara object.
	# This method is called at the start of each keyframe insertion before BOIDs are tested
	# to be in/out of the zone, because scene objects can move about during the animation.
	# If there is no Carrara object assigned to this zone, then the currently set values
	# are used.
	#------------------------------------------------------------------------------------
		if self.container_name == '':
			return

		center = self.container_c3dptr.get_translation("G") # Use global coordinates

		# Get how much to scale the zone by finding % of current scale to initial scale
		#print self.container_name
		scale = self.container_c3dptr.get_anim_param_value(current_time, scaling_str)/self.container_scale
		if scale == 0.0:
			scale = 1.0
		debug_line(self.container_name, ' center at ', center*conv, ' and scale = ', scale*conv)

		# Get current position
		if self.container_type == 'B':		# Box
			base = center - (self.container_size / 2.0)
		elif self.container_type == 'C':	# Cylinder
			base = center
			base.z = self.container_size.z / 2.0
		elif self.container_type == 'S':	# Sphere
			base = center

		# Set the values
		self.container_base = base
		self.container_size = self.orig_size * scale 
		self.container_radius = self.orig_radius * scale 
		self.container_height = self.orig_height * scale 

	#====================================================================================
	def inside(self, boid_loc):
	# Returns True if the parameter location is inside this container.
	#------------------------------------------------------------------------------------
	# Pseudocode:
	#	in = False
	#	if type = Box
	#		
	#	elif type = Cylinder
	#		
	#	else # type = Sphere
	#		
	#	
	#------------------------------------------------------------------------------------
		return

	#====================================================================================
	def to_safety(self, boid_loc):
	# This method returns a Vector which is the shortest path from the parameter location
	# into the safe container. If the location is within the container, a null vector is
	# returned. If the container is restricted, a null vector is returned.
	# If the container is restricted mode, an exception is raised.
	#------------------------------------------------------------------------------------
	# Pseudocode:
	#	if mode == restricted
	#		raise exception
	#	initialize v
	#	if type == Box
	#		if outside x boundary
	#			set v.x to move into container
	#		if outside y boundary
	#			set v.y to move into container
	#		if outside z boundary
	#			set v.z to move into container
	#	elif type == Cylinder
	#		compute lateral (x,y) direction to move back inside
	#		compute z direction if outside boundary
	#	else # type == Sphere
	#		if outside sphere
	#			set v to vector towards center
	#	return v * self.container_adj
	#------------------------------------------------------------------------------------
		if not self.container_safemode:
			raise ModeError('to_safety called but not a safe zone.')

		v = Vector()
		if self.container_type == 'B':
			if boid_loc.x < self.container_base.x:
				v.x = self.container_base.x - boid_loc.x
			if boid_loc.x > (self.container_base.x+self.container_size.x):
				v.x = (self.container_base.x + self.container_size.x) - boid_loc.x
			if boid_loc.y < self.container_base.y:
				v.y = self.container_base.y - boid_loc.y 
			if boid_loc.y > (self.container_base.y+self.container_size.y):
				v.y = (self.container_base.y + self.container_size.y) - boid_loc.y
			if boid_loc.z < self.container_base.z:
				v.z = self.container_base.z - boid_loc.z
			if boid_loc.z > (self.container_base.z+self.container_size.z):
				v.z = (self.container_base.z + self.container_size.z) - boid_loc.z
			#print 'Box container: v=', v*conv
		elif self.container_type == 'C':
			diff = self.container_base - boid_loc
			# Determine lateral distance to safety
			if limit_2D:
				diff.z = 0.0
			d = diff.mag() - self.container_radius
			if d > 0.0:
				v = diff.unit() * d
			# Determine z distance to safety
			if boid_loc.z < self.container_base.z:
				v.z = self.container_base.z - boid_loc.z
			if boid_loc.z > (self.container_base.z+self.container_height):
				v.z = (self.container_base.z + self.container_height) - boid_loc.z
		else: # self.container_type == 'S'
			diff = self.container_base - boid_loc
			if limit_2D:
				diff.z = 0.0
			d = diff.mag() - self.container_radius
			# If inside the sphere
			if d < 0.0:	
				return v
			else:
				v = diff.unit() * d

		return v * self.container_adj

	#====================================================================================
	def from_harm(self, boid_loc):
	# This method returns a Vector which is the shortest path from the parameter location
	# out of the restricted container. If the location is not within the container, a 
	# null vector is returned. If the container is safe, an exception is raised.
	#------------------------------------------------------------------------------------
	# Pseudocode:
	#------------------------------------------------------------------------------------
	#	if mode == safe
	#		raise exception
	#	v = Vector()
	#	if type == Box
	#		# Calculate the distance to the closest side and return a vector to that location 
	#		# If any dimension is outside the box, then location is not with the zone, so can exit
	#	elif type == Cylinder
	#		# If above or below container, then exit
	#		# If lateral distance to edge of cylinder > radius, then exit
	#		# Location is within the cylinder
	#		if (d < dup) & (d < ddown):
	#			v = diff.unit() * d
	#		elif (dup < ddown):
	#			v.z = dup
	#		else:
	#			v.z = -ddown
	#	else # type == Sphere
	#		# If 
	#		diff = boid_loc - self.container_base
	#		d = self.container_radius - diff.mag()
	#		if d <= 0.0:
	#			return v
	#		else:
	#			# Location is within the sphere
	#			v = diff.unit() * d
	#	return v * self.container_adj
	#------------------------------------------------------------------------------------
		if self.container_safemode:
			raise ModeError('from_harm called but not a restricted zone.')

		v = Vector()

		if self.container_type == 'B':
			# If location is between container end points
			if self.container_base.x < boid_loc.x < (self.container_base.x+self.container_size.x):
				dx = magmin(self.container_base.x-boid_loc.x,self.container_base.x+self.container_size.x-boid_loc.x)
			else:
				return v
			if self.container_base.y < boid_loc.y < (self.container_base.y+self.container_size.y):
				dy = magmin(self.container_base.y-boid_loc.y,self.container_base.y+self.container_size.y-boid_loc.y)
			else:
				return v
			if self.container_base.z < boid_loc.z < (self.container_base.z+self.container_size.z):
				dz = magmin(self.container_base.z-boid_loc.z,self.container_base.z+self.container_size.z-boid_loc.z)
			else:
				return v

			# Select the closest exit point from the dimensional distances computed above
			if limit_2D:
				if abs(dx) < abs(dy):
					v.x = dx
				else:
					v.y = dy
			else:
				# Set v to minimum distance of dx, dy, dz
				if (abs(dx) < abs(dy)) & (abs(dx) < abs(dz)):
					v.x = dx
				elif (abs(dy) < abs(dz)):
					v.y = dy
				else:
					v.z = dz
			#print 'Inside restricted zone from ', self.container_base*conv, ' to ', 
			#print (self.container_base+self.container_size)*conv, ' at '
			#print boid_loc*conv, ' with exit ', v*conv

		elif self.container_type == 'C':
			if not (self.container_base.z < boid_loc.z <= (self.container_base.z+self.container_height)):
				return v

			# Not above or below, so check to see if within the cylinder laterally
			diff = boid_loc - self.container_base
			diff.z = 0.0
			d = self.container_radius - diff.mag()
			if d <= 0.0:
				return v

			# Location is within the cylinder
			# If limited to 2D, then use the d vector
			if limit_2D:
				v = diff.unit() * d
			else:
				dup = self.container_base.z+self.container_height - boid_loc.z
				ddown = boid_loc.z - self.container_base.z
				if (d < dup) & (d < ddown):
					v = diff.unit() * d
				elif (dup < ddown):
					v.z = dup
				else:
					v.z = -ddown

		else: # self.container_type == 'S'
			diff = boid_loc - self.container_base
			d = self.container_radius - diff.mag()
			if d <= 0.0:
				return v
			else:
				# Location is within the sphere
				if limit_2D:
					diff.z = 0.0
				v = diff.unit() * d

		return v * self.container_adj * 3.0

	#------------------------------------------------------------------------------------

##########################################################################################


##########################################################################################
class CONTAINER_set:
#=========================================================================================
# This class structure is designed to support working with multiple containers (regions)
# each defining a safe or restricted region of space. Container instances are saved in
# a list of containers. Methods in this class handle containers as a whole.
#=========================================================================================

	#====================================================================================
	def __init__(self):
	#------------------------------------------------------------------------------------

		self.container_list = []			# List of containers

		for c in range (0, num_containers):

			# Add the containers to the list
			self.container_list.append (CONTAINER (c_safemode[c], c_type[c], c_name[c], 
						c_base[c], c_radius[c], c_height[c], c_size[c], c_adj[c]));

	#====================================================================================
	def update_zones (self):
	# Updates zone information for this iteration of keyframe insertions
	#------------------------------------------------------------------------------------
		for c in self.container_list:
			c.update()


	#====================================================================================
	def nearest_safe (self, boid_loc):
	# This method returns a Vector to the nearest safe region (container).
	# If location is within a safe region, a null vector is returned.
	# If there are no safe zones, a null vector is returned.
	#------------------------------------------------------------------------------------
	# Pseudocode:
	#	initialize entry = Vector()
	#	for all zones
	#		if mode is safe
	#			Find the shortest distance to safety in this container
	#			If this one is closer, replace entry with this path
	#	return entry
	#------------------------------------------------------------------------------------
		count = 0
		entry = Vector()
		for c in self.container_list:
			if c.container_safemode:
				# Find the shortest distance to safety in this zone
				safety = c.to_safety(boid_loc)

				# If this one is closer, replace entry with this path
				if (count == 0) | (safety.mag() < entry.mag()):
					entry = safety
				count += 1

		#if entry.mag() > 0.0:
		#	print 'Nearest safe location for ', boid_loc*conv, ' is ', entry*conv
		return entry


	#====================================================================================
	def closest_exit (self, boid_loc):
	# This method returns a Vector to the closest exit point for all restricted zones
	# the location is within. This vector is determined by summing the individual vectors
	# for each restricted zone inside.
	# The individual vectors have already been modified by the containers' strength values.
	# If location is not within a restricted zone, a null vector is returned.
	# If there are no restricted zones, a null vector is returned.
	#------------------------------------------------------------------------------------
	# Pseudocode:
	#	initialize exit = Vector()
	#	for all zones
	#		if mode is restricted
	#			add the distance vector out of harm to the exit vector
	#	return exit
	#------------------------------------------------------------------------------------
		exit = Vector()
		for c in self.container_list:
			if not c.container_safemode:
				exit += c.from_harm(boid_loc)

		#if exit.mag() > 0.0:
		#	print 'Closest exit at ', exit*conv
		return exit


	#====================================================================================
	def contain (self, boid_loc):
	# This method examines all of the containers against the location passed in.
	# If the location is within a safe region and is not inside a restricted region,
	# a null Vector is returned.
	# If the location is within a safe region and within a restricted region, it returns
	# a Vector that is the shortest path out of the restricted region. If it is within
	# more than one restricted region, it sums the Vectors of each one and returns that.
	# If the location is not within a safe region and not within a restricted region,
	# it returns a Vector to the edge of the nearest safe region.
	# If the location is not within a safe region is within a restricted region, it
	# sums the Vector of each and returns that.
	#------------------------------------------------------------------------------------
	# Pseudocode:
	#	return nearest_safe(boid_loc)+closest_exit(boid_loc)
	#------------------------------------------------------------------------------------
		return self.nearest_safe(boid_loc) + self.closest_exit(boid_loc)


	#------------------------------------------------------------------------------------



##########################################################################################


##########################################################################################
class BOID:
#=========================================================================================
# This class handles all methods and operations for each specific BOID in the system
#=========================================================================================

	#------------------------------------------------------------------------------------
	# Variables common to objects of this class

	boid_count = 0					# Current number of BOID objects

	#------------------------------------------------------------------------------------
	def __init__(self, name):

		# Initialize any class-local variables
		BOID.boid_count += 1
		self.number = BOID.boid_count	# Unique identifier for this BOID

		try:
			#print 'Initializing ' + name
			self.c3d_ptr = c3d.Object(name)
		except:
			raise ObjectError('***PySwarm was unable to find a BOID named ' + name + '***')

		# Initialize any instance variables that are unique for each BOID


		# Initialize location, heading, velocity, etc. of the BOID

		self.location = Vector()		# Current location 
		self.next_location = Vector()	# Extrapolates current location with current velocity
		self.heading = 0.0			# Current heading in radians
		self.velocity = Vector()		# Current velocity
		self.vel_change = Vector()		# Current velocity change
		self.land_timer = 0.0			# Current timer (if landed)
		self.just_landed = False		# True when the BOID strikes the ground

	#====================================================================================
	def read_position(self):
	# This method returns the BOID's current location
	#------------------------------------------------------------------------------------
		return self.c3d_ptr.get_translation("G") # Use global coordinates

	#====================================================================================
	def write_position(self):
	# This method draws or positions the BOID at the location currently assigned
	#------------------------------------------------------------------------------------
		self.c3d_ptr.set_translation(shifting(self.location), "G")

	#====================================================================================
	def read_rotation(self):
	# This method reads the BOID's current rotational values
	#------------------------------------------------------------------------------------
		rotation = self.c3d_ptr.get_rotation("L")
		# rotation contains the angles for (X=Banking; Y=Pitching; Z= Heading)
		r = rotation * 1.0
		#if (boid_orientation == 0):	# Fwd: +X; Up: +Z
			# Use r as it is

		if (boid_orientation == 1):	# Fwd: +Y; Up: +Z
			#r.x = rotation.y
			#r.y = -rotation.x
			r.z = rotation.z + 90.0

		elif (boid_orientation == 2):	# Fwd: -X; Up: +Z
			r.x = -rotation.x
			r.y = -rotation.y
			r.z = rotation.z + 180.0

		elif (boid_orientation == 3):	# Fwd: -Y; Up: +Z
			r.x = -rotation.y
			r.y = rotation.x
			r.z = rotation.z - 90.0

		elif (boid_orientation == 4):	# Fwd: +Z; Up: +Y
			r.x = -rotation.y
			r.y = rotation.x - 90.0
			r.z = rotation.z - 90.0

		elif (boid_orientation == 5):	# Fwd: +Z; Up: +X
			r.x = -rotation.y - 90.0
			r.y = rotation.z
			r.z = rotation.x + 180.0

		return r

	#====================================================================================
	def write_rotation(self,r):
	# This method rotates the BOID using the provided Vector() parameter. Values are
	# in degrees (PyCarrara's internal unit of measure).
	#------------------------------------------------------------------------------------
		rotation = r * 1.0
		#if (boid_orientation == 0):	# Fwd: +X; Up: +Z
			# Use rotation as it is

		if (boid_orientation == 1):	# Fwd: +Y; Up: +Z
			rotation.x = -r.y
			rotation.y = r.x
			rotation.z = r.z - 90.0

		elif (boid_orientation == 2):	# Fwd: -X; Up: +Z
			rotation.x = -r.x
			rotation.y = -r.y
			rotation.z = r.z + 180.0

		elif (boid_orientation == 3):	# Fwd: -Y; Up: +Z
			rotation.x = r.y
			rotation.y = -r.x
			rotation.z = r.z + 90.0

		elif (boid_orientation == 4):	# Fwd: +Z; Up: +Y
			rotation.x = r.y + 90.0
			rotation.y = -r.x
			rotation.z = r.z + 90.0

		elif (boid_orientation == 5):	# Fwd: +Z; Up: +X
			rotation.x = (r.z - 180.0)
			rotation.y = -90.0 - r.x
			rotation.z = r.y

		# r contains the angles for (X=Banking; Y=Pitching; Z= Heading)
		debug_line('write_rotation: r = ', r, '; rotation = ', rotation)
		self.c3d_ptr.set_rotation(shifting(rotation), "L")

	#====================================================================================
	def name(self):
	#------------------------------------------------------------------------------------
		return self.c3d_ptr.get_fullname()

	#====================================================================================
	def print_debug_info(self):
	#------------------------------------------------------------------------------------
		debug_line('BOID information for ', self.name())
		debug_line('  Location = ', self.location*conv)
		debug_line('  Next location = ', self.next_location*conv)
		debug_line('  Heading = ', self.heading*r2d)
		debug_line('  Velocity = ', self.velocity*conv)
		debug_line('  Vel Change = ', self.vel_change*conv)
		debug_line('  Land timer = ', self.land_timer)
		debug_line('  Just landed = ', self.just_landed)


	#====================================================================================
	def cohesion(self):
	# Rule 1: COHESION - Boids try to fly towards the center of mass of neighbouring boids.
	# The 'center of mass' is simply the average location of all the boids. I use the term 
	# center of mass by analogy with the corresponding physical formula (however we ignore 
	# individual masses here and treat all boids having the same mass).
	#
	# Landed Boids are ignored in calculating the center of mass. Only those currently in 
	# the "flock" are considered.
	#
	# With V0.5, COHESION is also influenced by the localization parameter.
	#------------------------------------------------------------------------------------

		# If this rule is not active, return a null vector
		if not activate_cohesion:
			return Vector()

		center = Vector()
		boid_count = 0

		for b in boids.boid_list:
			# Ignore self and BOIDs that have landed
			if (not (b.number == self.number)) & (b.land_timer == 0.0):
				# Ignore BOIDs not within local area if Localize is set
				if (localize_on) & ((b.location-self.location).mag()>localize_radius):
					continue
				center = center + b.next_location
				boid_count += 1
 
		if boid_count == 0:
			return center		# Return a null vector if no BOIDs to be near
		else:
			center = center / (1.0*boid_count)
			return (center - self.next_location) * (cohesion_adj / 50.0)


	#====================================================================================
	def separation(self):
	# Rule 2: SEPARATION - Boids try to keep a small distance from other boids.
	# The purpose of this rule is to make sure boids don't collide into each other. 
	# Look at each boid, and if it's within a defined distance (measured in feet) 
	# of another boid move it as far away again as it already is. This is done by 
	# subtracting from a vector c the displacement of each boid which is near by. We 
	# initialise c to zero as we want this rule to give us a vector which when added 
	# to the current location moves a boid away from those near it.

	# With V0.5, SEPARATION is also influenced by the localization parameter.
	#------------------------------------------------------------------------------------

		# If this rule is not active, return a null vector
		if not activate_separation:
			return Vector()

		c = Vector()
		for b in boids.boid_list:
			if not (b.number == self.number):
				diff = b.next_location - self.next_location
				if diff.mag() < separation_distance:
        				c = c - diff.unit() * separation_distance

		return c * (separation_adj / 4.0)


	#====================================================================================
	def alignment(self):
	# Rule 3: ALIGNMENT - Boids try to match velocity with near boids.
	# This is similar to Rule 1, however instead of averaging the locations of the other 
	# boids we average the velocities. We calculate a 'perceived velocity' (pv), then add 
	# a small portion to the boid's current velocity.

	# Landed Boids are ignored in calculating ALIGNMENT. Only those currently flying in 
	# the "flock" are considered.

	# With V0.5, ALIGNMENT is also influenced by the localization parameter.
	#------------------------------------------------------------------------------------

		# If this rule is not active, return a null vector
		if not activate_alignment:
			return Vector()

		pv = Vector()
		boid_count = 0
		for b in boids.boid_list:
			# Ignore self and BOIDs that have landed
			if (not (b.number == self.number)) & (b.land_timer == 0.0):
				# Ignore BOIDs not within local area if Localize is set
				if (localize_on) & ((b.location-self.location).mag()>localize_radius):
					continue
				pv = pv + b.velocity
				boid_count += 1

		if boid_count == 0:
			# Force returning a null vector (no effect) if no BOIDs to align with
			return Vector()
		else:
			# Computer average of group velocity
			pv = pv / (1.0*boid_count)
			return (pv - self.velocity) * (alignment_adj / 12.0)


	#====================================================================================
	def containment(self):
	# Rule 4: CONTAINMENT - Boids tend to stay within a defined boundary.
	#------------------------------------------------------------------------------------

		# If this rule is not active, return a null vector
		if not activate_containment:
			return Vector()

		cv = containers.contain(self.next_location)
		#if cv.mag() > 0.0:
		#	print 'Container for ', self.name(), ' at time ', current_time
		#	print 'Container vector ', cv*conv, ' @ ', self.location*conv
		return cv

	#====================================================================================
	def attraction(self):
	# Rule 5: ATTRACTOR - Boids tend to move towards a specified object in the scene.
	# Create a vector to the attractor object, and return that vector adjusted by the force
	# to be applied. The farther away the BOID is from the attractor, the greater the force.
	# For negative strengths, the opposite is used. The farther away the BOID is, the weaker
	# the repulsion force.
	#------------------------------------------------------------------------------------

		# If this rule is not active, return a null vector
		if not activate_attraction:
			return Vector()

		# Calculate distance vector
		diff = attractor.get_anim_translation(current_time) - self.next_location

		# If attraction_distance used, exit if outside the radius
		if (attraction_distance != 0.0) & (diff.mag() > attraction_distance):
			return Vector()

		# If negative strength, use repulsion calculation
		if attraction_adj < 0.0:
			d = 1.0 / diff.mag()
			diff = diff.unit() * d * 5000.0

		return diff * (attraction_adj / 20.0)

	#====================================================================================
	def landing(self):
	# Rule 6: LANDING - Boids pause occasionally when they encounter the ground.
	# This function manages the landed timer if the BOID is landed. It returns a value of 
	# True when the timer has expired (so the caller can initiate a new velocity vector.
	#------------------------------------------------------------------------------------

		# If this rule is not active, return 
		if not activate_landing:
			return False

		#print 'Location:', self.location

		# If already landed, then decrement the timer and set back to 0.0 when done
		if self.land_timer > 0.0:
			self.land_timer -= 1.0 / fps		# Count the timer down
			if not (self.land_timer > 0.0):	# Reset to zero if done
				self.land_timer = 0.0
				# Tell caller the timer has expired and exit
				return True

		# Else if the boid's location is at or below the ground level
		elif self.location.z <= ground:

			# then set the boid's z location to ground level
			self.location.z = ground
			self.velocity = Vector()
			self.vel_change = Vector()
			just_landed = True

			# activate/set the timer to # seconds to remain landed
			var = random() * (max_landing_time - min_landing_time)
			self.land_timer = min_landing_time + var

			#print self.name(), ' just landed at ', current_time, ' for ',self.land_timer

		return False

	#====================================================================================
	def speedlimit(self):
	# After vc has been clipped and the new velocity value has been calculated, a final
	# check is made on the current velocity to ensure it is within defined constraints.
	# If the pitch of the velocity vector is outside pitch angle limits (min/max), the
	# velocity.z value is reduced. Then, if the speed is outside speed limits, this method
	# updates the velocity to be within limits. Note that increasing or decreasing the
	# length of the velocity vector to fit within speed limits does not affect pitch.
	#------------------------------------------------------------------------------------

		speed = self.velocity.mag()

		#----------
		# Clip velocity based on min/max pitching angles
		pitch = -asin (self.velocity.z / speed)
		debug_line('Pitch = ', pitch*r2d)

		# Check pitch angles and limit velocity.z value
		if (min_pitch_angle != 0.0) & (pitch < min_pitch_angle):
			self.velocity.z = -speed * sin(min_pitch_angle)
			debug_line('Velocity z increased.')
		elif (max_pitch_angle != 0.0) & (pitch > max_pitch_angle):
			self.velocity.z = -speed * sin(max_pitch_angle)
			debug_line('Velocity z decreased.')

		# Recomputer speed again
		speed = self.velocity.mag()

		#----------
		if (min_speed > 0.0) & (speed < min_speed):
			self.velocity = self.velocity.unit() * min_speed
			debug_line('Speed increased to min speed.')

		elif (max_speed > 0.0) & (speed > max_speed):
			self.velocity = self.velocity.unit() * max_speed
			debug_line('Speed decreased to max speed.')


	#====================================================================================
	def vc_clip(self,vc):
	# Before changing the velocity vector, limit the heading change possible.
	# Parameters used to adjust vc are (in order of application):
	#  * max_acceleration - the magnitude of vc cannot exceed this amount
	#  * max_turn - the heading change cannot exceed this value
	#
	# The result of this function is to return a vector with the same speed 
	# (vector length), but restricting the angle (heading) to not exceed the "max_turn" 
	# value. This is done by computing the heading change, comparing it to the max amount 
	# allowed, and reducing the heading change if it exceeds that max amount.
	#------------------------------------------------------------------------------------

		debug_line('Max acceleration check with vc = ', vc)

		# First, clip vc if max acceleration is exceeded
		if (max_acceleration > 0.0) & (vc.mag() > (max_acceleration/fps)):
			vc = vc.unit() * max_acceleration / fps
			debug_line('vc clipped to ', vc)

		if max_turn == 0.0:
			return vc

		debug_line('Max turn check with current heading = ', self.heading*r2d)

		# Velocity values are in internal metrics
		#v1 = self.velocity			# Previous iteration's velocity
		#current_heading = atan2 (self.velocity.y, self.velocity.x)
		v2 = self.velocity + vc				# This iteration's velocity
		new_heading = atan2 (v2.y, v2.x)

		hc = (new_heading - self.heading) * r2d	# hc is now in degrees
		# Adjust for when angles cross +/- 180 degrees
		if (hc < -pi):
			hc += twopi
		elif (hc > pi):
			hc -= twopi
		debug_line('New heading = ', new_heading*r2d, ' with heading change = ', hc)

		# If heading change (defined in degrees/frame) exceeds the turning limit 
		if abs(hc) > max_turn:

			# Clip the velocity change vector accordingly
			vc *= max_turn / abs(hc)	# This is a unitless number

			# Next set of lines are for debugging
			v2 = self.velocity + vc
			new_heading = atan2 (v2.y, v2.x)
			hc = (new_heading - self.heading) * r2d
			debug_line('Max turn exceeded. vc clipped to ', vc, ' with heading change = ', hc)

		return vc


#=========================================================================================

##########################################################################################
class CAMERA:
#=========================================================================================
# This class processes the camera and camera focus objects in the Carrara scene.
#=========================================================================================
	#------------------------------------------------------------------------------------
	# class variables

	#====================================================================================
	def __init__(self):
	# Initialize the class by creating the camera and focus objects.
	# This method also clears out keyframes if the flag is set
	#------------------------------------------------------------------------------------
		if len(camera_name) > 0:
			try:
				self.camera = c3d.Object ('/' + camera_name)
			except:
				raise ObjectError('***PySwarm was unable to find a camera named ' 
							+ camera_name + '***')
			print 'Camera ', camera_name, ' found.'

		if len(camera_focus_name) > 0:
			try:
				self.camera_focus = c3d.Object ('/' + camera_focus_name)
			except:
				raise ObjectError('***PySwarm was unable to find a camera focus named ' 
							+ camera_focus_name + '***')
			print 'Camera focus ', camera_focus_name, ' found.'

		if clear_key_frames:
			# Clear out all keyframes that exist for the camera and its focus
			if len(camera_name) > 0:
				self.camera.clear_keyframes('*')
			if len(camera_focus_name) > 0:
				self.camera_focus.clear_keyframes('*')

	#====================================================================================
	def draw (self, focus_loc):
	# This method moves the camera and focus objects.
	#------------------------------------------------------------------------------------
		# Position the camera
		if len(camera_name) > 0:
			self.camera.set_keyframe_translation (current_time, focus_loc+camera_offset)

		# Set the position of the camera focus
		if len(camera_focus_name) > 0:
			self.camera.set_keyframe(current_time, rotation_str, self.pointAt(self.camera_focus.get_translation()))
			#self.camera_focus.set_keyframe_translation (current_time, focus_loc)
			#print 'Focus center = ', focus_loc*conv
		else:
			self.camera.set_keyframe(current_time, rotation_str, self.pointAt(focus_loc))


	#====================================================================================
	def pointAt (self, focus_obj):
	# This method rotates the camera towards the focus object.
	#------------------------------------------------------------------------------------
		# Subtract Camera position from focus object
		pointAtVector = focus_obj - self.camera.get_translation()
		# return rotation vector
		return Vector(0.5*pi+asin(pointAtVector.z / pointAtVector.mag()), 0.0, -atan2(pointAtVector.x, pointAtVector.y)) * r2d

##########################################################################################
class BOID_set:
#=========================================================================================
# This class processes the set of BOID instances.
#=========================================================================================

	#====================================================================================
	def __init__(self):
	# Initialize the class by creating the BOIDs and adding them to the list
	# This method also clears out keyframes if the flag is set
	#------------------------------------------------------------------------------------

		#debug_line('Initializing boid set')

		# Initialize all BOIDs
		self.boid_list = []			# BOIDs list
		self.center = Vector ()

		for b in range (0, num_boids):
			name = '/' + boids_name + '%i' %(b+1)
			boid = BOID(name)			# Create the BOID and retain a pointer
			self.boid_list.append(boid)	# Add BOID to list

			if clear_key_frames:
				#print 'Clearing frames: ', name
				boid.c3d_ptr.clear_keyframes('*')

			# Set BOID location based on option selected
			# (1) use the current placement of BOIDs
			if init_position_opt == 1:
				s = boid.read_position()

			# (2) randomly place BOIDs inside a container 
			elif init_position_opt == 2:
				# Positioning is based on the type of bounding area being used
				# If no bounding area is defined, then what???
				# Use container #1 (first entry) for placing, so get a pointer to it
				c = containers.container_list[0]
				# Repeat the following until a safe point is found
				d = Vector(1,1,1)
				while d.mag() != 0.0:
					s = c.container_base * 1.0			# Start location with base of area
					if c.container_type == 'C':		# Cylinder
						angle = random() * twopi
						r = c.container_radius * random()
						s += Vector(sin(angle)*r, cos(angle)*r, c.container_height*random())

					elif c.container_type == 'S':	# Sphere
						angle1 = random() * twopi
						angle2 = acos(2*random()-1) #random() * twopi
						r = c.container_radius * random() # Random radius
						s += Vector(cos(angle1)*sin(angle2), sin(angle1)*sin(angle2), cos(angle2)) * r

					else: # (assume 'B' - Box)
						s.x += random() * c.container_size.x
						s.y += random() * c.container_size.y
						s.z += random() * c.container_size.z
					d = containers.closest_exit(s)

			# (3) randomly place BOIDs inside a sphere 
			else: # init_position_opt == 3
				s = init_center				# Start location with base of area
				d = Vector(1,1,1)
				while d.mag() != 0.0:
					angle1 = random() * twopi
					angle2 = acos(2*random()-1) #random() * twopi
					r = init_radius * random() # Random radius
					s += Vector(cos(angle1)*sin(angle2), sin(angle1)*sin(angle2), cos(angle2)) * r
					d = containers.closest_exit(s)

			if limit_2D:					# If limited to 2D
				s.z = init_height		   	# then keep z value always at init_height

			boid.location = s				# Set BOID location
			debug_line('Initiating ',name,' at location ',s*conv)

			# Compute velocity based on option selected
			# (1) Set velocity vector based on direction BOIDs are facing & initial speed value (speed)
			if init_velocity_opt == 1:
				# Get rotations of the individual and convert to radians
				#w = boid.c3d_ptr.get_rotation("G") / r2d
				w = boid.read_rotation() / r2d
				#print('Initial heading for ', name,' is ', w*r2d)

				# rotation angles in w represent Vector(bank,pitch,heading) where:
				# 	heading = w.z
				# 	pitch = w.y
				#	bank is not important here, so use 0.0
				#	*** A later version may use banking to adjust the velocity vector...
				v = Vector(cos(w.z), sin(w.z), sin(w.y))

				# And calc velocity vector using initial speed
				v = v.unit() * init_speed
				#print 'Initial velocity set to ', v*conv	

			# (2) Set velocity vector randomly
			elif init_velocity_opt == 2:
				angle1 = random() * twopi
				angle2 = (random()*60.0 - 30.0) / r2d # pitch limited to +/- 30 degrees
				v = Vector(cos(angle1)*sin(angle2), sin(angle1)*sin(angle2), 1.0-cos(angle2))
				v = v.unit() * init_speed

			# (3) Set velocity vector using a predefined vector for all individuals
			else: 						# init_velocity_opt == 3
				# Compute the velocity vector from the provided heading and pitch values
				angle1 = init_heading
				angle2 = init_pitch
				motion_vector = Vector (cos(angle1),sin(angle1),sin(angle2))
				v = motion_vector.unit() * init_speed

			if limit_2D:					# If limited to 2D
				v.z = 0.0					# then no velocity in z direction

			# If initiating BOIDs in LANDED mode
			if (activate_landing) & (initiate_landed):
				# Initiate timers for this BOID
				boid.land_timer = min_landing_time + random() * (max_landing_time - min_landing_time)
				boid.just_landed = True
				#debug_line (boid.name(),' timer initialized to ',boid.land_timer)
				boid.next_location = s
			else:
				boid.velocity = v				# Set BOID velocity
				#debug_line ('Initiating ',name,' velocity to ',v*conv)
				boid.next_location = s + (v/fps)

			# Set the initial BOID heading in radians
			boid.heading = atan2(v.y,v.x)

	#====================================================================================
	def draw (self):
	# This method calls the procedure to set the boids' location and rotation values
	# based on current position, velocity, and velocity change
	#------------------------------------------------------------------------------------

		# This variable computes the BOID center to position the camera_focus object
		self.center = Vector()			# Initialize the variable
		boid_count = 0

		# Step through all of the BOIDs
		for b in self.boid_list:
			self.center += b.location
			boid_count += 1

			# Position BOID
			# If BOID landed for the next iteration and BOID did not just land
			#if (b.land_timer > 1.0/fps) & (not b.just_landed):
			#	continue		# Then skip this BOID

			# If first frame, no need to position BOIDs
			#if frame_count == 1:
			#	continue

			b.write_position()

			# Apply pitch and bank to the Boid based on current velocity vector
			if b.velocity.mag() > 0.0:

				# Heading determination
				_heading = atan2 (b.velocity.y, b.velocity.x)

				# Pitch determination
				_pitch = -asin (b.velocity.z / b.velocity.mag()) * pitching_factor

				# Bank determination and limit banking
				hc = b.heading - _heading
				# Adjust for when angles cross +/- 180 degrees
				if (hc < -pi):
					hc += twopi
				elif (hc > pi):
					hc -= twopi
				_bank = (hc) * fps * banking_factor
				if (max_bank_angle != 0.0) & (abs(_bank) > max_bank_angle):
					if _bank < 0.0:
						_bank = -max_bank_angle
					else:
						_bank = max_bank_angle

			# Otherwise, No velocity, use direction pointed for heading. Zero out pitch and bank.
			else:
				#_heading = b.c3d_ptr.get_rotation("G").z / r2d
				_heading = b.read_rotation().z / r2d
				_pitch = 0.0
				_bank = 0.0

			b.just_landed = False

			rotation = Vector(_bank, _pitch, _heading) * r2d

			# Replace the heading value with the new one and rotate into position
			b.heading = _heading
			b.write_rotation(rotation)

		if boid_count > 0:
			debug_line ('Center = ', self.center*conv, '# ', boid_count)
			self.center /= (1.0*boid_count)

	#====================================================================================
	def move (self):
	# This method is the engine of the simulation by calculating the effects of all the
	# rules for each boid, and applying those effects to determine the boids' new locations.
	#------------------------------------------------------------------------------------

		debug_line ('Boids Move() function called.')

		# Step through all of the BOIDs
		for b in self.boid_list:

			debug_line (b.name())

			# Apply Rule #6
			# If timer has just expired
			if b.landing():
				debug_line ('Processing take-off for ', b.name(), ' at ', current_time)
				b.vel_change = Vector()	
				angle1 = b.heading

				# Calc vectors for lift-off. Use the basic formula to determine unit vector
				b.velocity = Vector(cos(angle1)*cos(takeoff_angle),
							    sin(angle1)*cos(takeoff_angle),
							    sin(takeoff_angle)) * init_speed

			# If still landed, clear out velocity. We're done with this BOID
			elif b.land_timer > 0.0:
				debug_line (b.name(), ' still landed at ', current_time)

				# No need to calculate new BOID location. Go on to the next
				continue

			# Otherwise, do basic processing to calculate new location and velocity of the BOID
			else:
				# Determine the velocity change based on applying Rules #1, 2, 3, 4, and 5
				velocity_change = b.cohesion() + b.separation() + b.alignment() + b.containment() + b.attraction()

				# Use vc_clip to clip the velocity change if needed
				# and save the velocity change vector for later reference
				b.vel_change = b.vc_clip(velocity_change)

				# Calculate the new velocity
				b.velocity += b.vel_change

				# If limited to 2D, then no velocity in z direction
				if limit_2D:
					b.velocity.z = 0.0
				debug_line ('Velocity of ', b.name(), b.velocity)

				# Check for speed limit
				b.speedlimit()

			# Set the new location by adding motion based on velocity for this time period
			b.location += b.velocity / fps
			debug_line ('Boid ', b.name(), ' location set to ', b.location*conv)
			b.next_location = b.location + (b.velocity / fps)

			b.print_debug_info()

	#------------------------------------------------------------------------------------

#=========================================================================================



##########################################################################################
# PySwarm's main section
#=========================================================================================

print 'Starting PySwarm V0.6'

start_debug()

if activate_attraction:   # An attractor name was provided
	try:
		attractor = c3d.Object('/'+attractor_name)
	except:
		raise ObjectError('***PySwarm was unable to find the attractor object named ' 
					+ attractor_name + '***')

# Initialize the camera and focus class
camera = CAMERA()

# Initialize timer data
start_run = datetime.now()
progress = 1

# Needed to initiate BOIDs properly
T = c3d.get_time()
current_time = sim_start_time
c3d.set_time (current_time)

total_frames = int(fps*sim_end_time) - int(fps*sim_start_time) + 1
frame_count = 0.0

# Initialize the CONTAINER_set instance
containers = CONTAINER_set()

# Initialize the BOID_set instance
boids = BOID_set()
#for b in boids.boid_list:
#	print b.number, b.name()
print len(boids.boid_list), ' BOIDs found.'

# Loop through each keyframe in simulation
for keyframe in range (int(fps*sim_start_time),int(fps*sim_end_time)+1):
	frame_count += 1.0
	duration = datetime.now() - start_run

	# Check to see if it is time to print a progress update (every 5 seconds)
	if duration.seconds == progress:
		if (progress % 5) == 0:
			percent_complete = frame_count / total_frames
			print('\nProcess is %i percent complete.' %int(percent_complete*100))
		else:
			print '>',
		progress += 1

	# Update the current sim time (in seconds)
	current_time = keyframe *1.0 / fps
	c3d.set_time (current_time)
	#debug_line ('Current time ',current_time)

	containers.update_zones()		# Update any changes to Carrara objects used as zones

	boids.draw()				# Draw boids at that location
	camera.draw(boids.center)		# Reposition the camera and focus if needed
	boids.move()				# Then update the location

end_debug()

duration = datetime.now() - start_run
keyframes = frame_count * num_boids
kfps_rate = keyframes / duration.seconds + duration.microseconds/1000000.0
print ''
print frame_count, ' keyframe intervals inserted for time ', 
print sim_start_time, ' to ', sim_end_time, ' seconds.'
print kfps_rate, ' keyframes inserted per second.'
print '\nPySwarm V0.6 ended normally.'
print 'Total time: %8.4f seconds.' %(duration.seconds + duration.microseconds/1000000.0)

# Reset the time back to when PySwarm was imported
c3d.set_time(T)

##########################################################################################
